#include "Includes.h"
///////////////////////////////////////////////////////////////////////////////////////
int GBL_ForwardSonic_Move_Dist_mm_x=0;
int GBL_ForwardSonic_Move_Max_Speed=0;
int GBL_ForwardSonic_Move_Min_Speed=0;
int GBL_ForwardSonic_Move_Gyro_Delta_Max_Speed=0;
int GBL_ForwardSonic_Move_Exit_Time=0;
TTimers GBL_ForwardSonic_Move_Timer_x=0;

int GBL_ForwardSonic_Move_exit_ahead=0;

#define GBL_ForwardSonic_Move_exit_ALLOWED 1



task Forward_Dist_BSonar_Gyro_task()
{
	float Kp_x=0.5,Ki_x=0.0005,Kd_x=0.001;
	float Output_x=0;
	int Last_Err_x=0,Err_x=0;
	float Integral_x=0,Derivation_x=0;

	//gyro
	float Kp_gyro=0.75,Ki_gyro=0.0002,Kd_gyro=0.01;
	float Output_gyro=0;
	int Last_Err_gyro=0,Err_gyro=0;
	float Integral_gyro=0,Derivation_gyro=0;

	int Aim_gyro=0;
	int Aim_Ticks_x= GBL_ForwardSonic_Move_Dist_mm_x; //enc_B_L
	int temp=0;

	float dist_b_now=SensorValue[son_B];
	float dist_b_last=dist_b_now;

	ClearTimer(GBL_ForwardSonic_Move_Timer_x);
	while(time1[GBL_ForwardSonic_Move_Timer_x]<GBL_ForwardSonic_Move_Exit_Time)
	{
		//y
		temp=dist_b_now;
		dist_b_now=SensorValue[son_B];
		if(dist_b_now>dist_b_last*1.7)
		{
			wait1Msec(1);
			continue;
		}
		else
			dist_b_last=dist_b_now;

		Err_x=Aim_Ticks_x- dist_b_now;
		if(abs(Err_x)<10) //blind area
			Err_x=0;
		else if(abs(Err_x)<20)
			Integral_x+=Err_x;

		Derivation_x=Err_x-Last_Err_x;
		Output_x=Kp_x*Err_x+Ki_x*Integral_x+Kd_x*Derivation_x; //calc Output_x
		Last_Err_x=Err_x;

		if(abs(Output_x)>=GBL_ForwardSonic_Move_Max_Speed) // limit Output_x
			Output_x=sgn(Output_x)*GBL_ForwardSonic_Move_Max_Speed;

		if(abs(Output_x)<GBL_ForwardSonic_Move_Min_Speed)
			Output_x=sgn(Output_x)*GBL_ForwardSonic_Move_Min_Speed;

		//gyro
		Err_gyro=Aim_gyro - SensorValue[gyro];
		if(abs(Err_gyro)<=5) //blind area
			Err_gyro=0;
		else if(abs(Err_gyro)<=25)
			Integral_gyro+=Err_gyro;
		Derivation_gyro=Err_gyro-Last_Err_gyro;
		Output_gyro=Kp_gyro*Err_gyro+Ki_gyro*Integral_gyro+Kd_gyro*Derivation_gyro; //calc Output_x
		Last_Err_gyro=Err_gyro;
		if(abs(Output_gyro)>=GBL_ForwardSonic_Move_Gyro_Delta_Max_Speed) // limit Output_gyro
			Output_gyro=sgn(Output_gyro)*GBL_ForwardSonic_Move_Gyro_Delta_Max_Speed;

		motor[Mtr_R_F]= -Output_x + Output_gyro ;
		motor[Mtr_L_F]=  Output_x + Output_gyro ;
		motor[Mtr_L_B]=  Output_x + Output_gyro ;
		motor[Mtr_R_B]= -Output_x + Output_gyro ;
		wait1Msec(1);

		/******************************************************************************************
		//  exit ahead
		*****/
		if(GBL_ForwardSonic_Move_exit_ahead==1 && GBL_ForwardSonic_Move_exit_ALLOWED==1)
		{
			if( Err_x==0 || dist_b_now>=Aim_Ticks_x )
			{
				writeDebugStream("exit:");
				break;
			}
		}

	}
	writeDebugStream("sonar:%d ,%d ,%d\n",time1[GBL_ForwardSonic_Move_Timer_x],Aim_Ticks_x,temp);
	Move_Stop();
}
/////////////////////////////////////////////////////
void Forward_Dist_BSonar_Gyro_TASK_FUN(int Dist_mm_x,int Max_Speed,int Min_Speed, int Gyro_Delta_Max_Speed,int Exit_Time,TTimers Timer_x,int exit_ahead)
{
	GBL_ForwardSonic_Move_Dist_mm_x=Dist_mm_x;
	GBL_ForwardSonic_Move_Max_Speed=Max_Speed;
	GBL_ForwardSonic_Move_Min_Speed=Min_Speed;
	GBL_ForwardSonic_Move_Gyro_Delta_Max_Speed=Gyro_Delta_Max_Speed;
	GBL_ForwardSonic_Move_Exit_Time=Exit_Time;
	GBL_ForwardSonic_Move_Timer_x=Timer_x;
	GBL_ForwardSonic_Move_exit_ahead=exit_ahead;
	StartTask(Forward_Dist_BSonar_Gyro_task);
}
